﻿// 2020/6/18: 首个版本
// 2020/7/2: 添加目标物指标计算代码
// 2020/9/29: 修正calBoxCorners
// 2021/3/11: TTC, THW, DCA数值范围限制为20
// 2021/7/7: 新增objSensorSampleProtocols
// 2021/8/12: 新增rcs字段
// 2023/10/11: 补充部分英文注释
// 2023/10/27: 增加雷达径向速度字段
// 2023/10/30: 增加雷达切向速度字段
// 2023/11/15: 支持v7，增加z轴速度/加速度字段
// 2024/3/25: 修正注释
// 2025/4/30: 增加objSensorSampleConverters
// 2025/5/21: 修正heading插值。修正convObjSensorSampleV7
// 2025/8/7: 新增ObjectClass.WheelBarrow, TruckHead, TrafficSign, TrafficLight

var PositionMode = {
    ClosestPoint : 1, // 最近点
    BoxCenter : 2, // 框中心
};

var ObjectClass = {
    General : 1, // 一般物体大类
    Car : 2, // 车辆大类
    Pedestrian : 3, // 行人大类
    Truck : 4, // 货车大类
    Bike : 5, // Bike大类
    RailCar : 6, // 轨道车辆大类
    Special : 7, // 特殊物体大类
    RoadStatic : 8, // 道路内静态物体大类
    SideStatic : 9, // 道路外静态物体大类

    GeneralSmall : 11, // 一般小物体
    GeneralBig : 12, // 一般大物体
    Barrier : 13, // 一般障碍物

    Van : 21, // 面包车
    Minibus : 22, // 小巴
    Bus : 23, // 大巴
    BatteryCart : 24, // 园区电瓶车
    TinyCar : 25, // 微型车
    SUV : 26, // SUV

    Adult : 31, // 成人
    Child : 32, // 小孩
    Scooter : 33, // 平衡车
    WheelChair : 34, // 轮椅
    WheelBarrow : 35, // 手推车

    Minitruck : 41, // 小卡车
    ContainerTruck : 42, // 货柜车（货柜车头 + 拖车）
    SpecialCar : 43, // 特种车辆
    Trailer : 44, // 拖车
    TruckHead : 45, // 货柜车头

    Motorbike : 51, // 摩托车
    Bicycle : 52, // 自行车
    ElectricBike : 53, // 电瓶自行车
    Tricycle : 54, // 三轮车

    Train : 61, // 火车
    Tram : 62, // 有轨电车

    Animal : 71, // 动物
    Ball : 72, // 球类
    Litter : 73, // 垃圾等杂物

    Cone : 81, // 锥形路障
    ManholeCover : 82, // 井盖
    Patch : 83, // 路面补丁
    Gantry : 84, // 龙门架

    Pole : 91, // 竖杆
    Tree : 92, // 树木
    Vegetation : 93, // 灌木
    Building : 94, // 建筑物
    TrafficSign : 95, // 交通标志
    TrafficLight : 96, // 交通灯
};

function ObjectColor() {
    this.valid = false;  // Whether the color is valid / 颜色是否有效
    this.r = 0;  // Red component of the object's color / 目标物的颜色R分量
    this.g = 0;  // Green component of the object's color / 目标物的颜色g分量
    this.b = 0;  // Blue component of the object's color / 目标物的颜色b分量
};

function ObjectInfo() {
    this.id = 0;  // ID of the object / 目标物ID
    this.age = 0;  // Age of the object / 目标物的Age
    this.rawID = null;  // Raw ID of the object / 目标物的原始ID
    this.rawAge = null;  // Raw age of the object / 目标物的原始Age
    this.confidence = null;  // [%] Confidence (existence) of the object / 目标物的置信度
    this.timeOffset = null;  // [us] Time offset (to the frame's time) / 时间偏置(相对于帧的时间戳)
    this.color = new ObjectColor();  // Color of the object / 目标物的颜色
    this.classification = ObjectClass.General;  // Classification of the object / 目标物的类别
    this.rawClassID = null;  // Raw classification ID of the object / 目标物的原始类别ID
    this.classConfidence = null;  // [%] Confidence (classification) of the object / 目标物的分类置信度
    this.posMode = PositionMode.ClosestPoint;  // Position mode of the object 目标物的位置模式
    this.posX = 0;  // [m] X-axis coordination of the object's position / 目标物的x轴方向位置
    this.posY = 0;  // [m] Y-axis coordination of the object's position / 目标物的y轴方向位置
    this.posZ = null;  // [m] Z-axis coordination of the object's position / 目标物的z轴方向位置
    this.posXSigma = null;  // [m] X-axis sigma of the object's position / 目标物x轴方向位置的精度
    this.posYSigma = null;  // [m] Y-axis sigma of the object's position / 目标物y轴方向位置的精度
    this.posZSigma = null;  // [m] Z-axis sigma of the object's position / 目标物z轴方向位置的精度
    this.cpx = 0;  // [m] X-axis coordination of the object's closest point / 目标物的最近点x轴坐标
    this.cpy = 0;  // [m] Y-axis coordination of the object's closest point / 目标物的最近点y轴坐标
    this.cpd = 0;  // [m] Distance between the object's closest point and subject vehicle's contour / 目标物的最近点与本车轮廓距离
    this.vxAbs = null;  // [KPH] X-axis absolute velocity of the object / x轴方向绝对速度
    this.vyAbs = null;  // [KPH] Y-axis absolute velocity of the object / y轴方向绝对速度
    this.vzAbs = null;  // [KPH] Z-axis absolute velocity of the object / z轴方向绝对速度
    this.axAbs = null;  // [m/s²] X-axis absolute acceleration of the object / x轴方向绝对加速度
    this.ayAbs = null;  // [m/s²] Y-axis absolute acceleration of the object / y轴方向绝对加速度
    this.azAbs = null;  // [m/s²] Z-axis absolute acceleration of the object / z轴方向绝对加速度
    this.vxRel = null;  // [KPH] X-axis relative velocity of the object / x轴方向相对速度
    this.vyRel = null;  // [KPH] Y-axis relative velocity of the object / y轴方向相对速度
    this.vzRel = null;  // [KPH] Z-axis relative velocity of the object / z轴方向相对速度
    this.axRel = null;  // [m/s²] X-axis relative acceleration of the object / x轴方向相对加速度
    this.ayRel = null;  // [m/s²] Y-axis relative acceleration of the object / y轴方向相对加速度
    this.azRel = null;  // [m/s²] Z-axis relative acceleration of the object / z轴方向相对加速度
    this.yawRate = null;  // [deg/s] Yaw rate of the object / 横摆角速度
    this.curvature = null;  // [1/m] Turning curvature of the object / 转弯曲率
    this.heading = null;  // [deg] Orientation of the object / 目标物的朝向
    this.width = null;  // [m] Width of the object's box / 目标物的宽度
    this.length = null;  // [m] Length of the object's box / 目标物的长度
    this.height = null;  // [m] Height of the object's box / 目标物的高度
    this.trajectory = [];  // List of object's trajectory points / 目标物轨迹点的列表
    this.contour = [];  // List of object's contour points / 目标物轮廓点的列表
    this.rcs = null;  // [dB] Radar-cross-section of the radar target / 雷达目标物的雷达散射截面
    this.vLng = null; // [m/s] Longitudinal velocity of the radar target / 雷达目标物的径向速度
    this.vLat = null; // [m/s] Lateral velocity of the radar target / 雷达目标物的切向速度
    this.speed = function () { // Get absolute speed / 获取绝对车速 [KPH]
        if (this.vxAbs != null && this.vyAbs != null) {
            if (this.vzAbs != null) return Math.sqrt(this.vxAbs * this.vxAbs + this.vyAbs * this.vyAbs + this.vzAbs * this.vzAbs);
            else return Math.sqrt(this.vxAbs * this.vxAbs + this.vyAbs * this.vyAbs);
        } else return null;
    }
    this.category = function () { // Get object's category / 获取目标物大类
        if (this.classification < 10) return this.classification;
        else return Math.floor(this.classification / 10);
    }
}

function ObjectSensorFov() {
    this.positionX = 0;  // [m] X-axis coordination of the FOV's center point / FOV中心点x轴坐标
    this.positionY = 0;  // [m] Y-axis coordination of the FOV's center point / FOV中心的y轴坐标
    this.angleRange = 90;  // [deg] Angle range of the FOV / FOV的角度范围
    this.orientation = 0;  // [deg] Central heading angle of the FOV FOV中轴线朝向角
    this.distanceRange = 100;  // [m] Detect range of the FOV / FOV探测距离范围
    this.blindRange = 0;  // [m] Blind range of the FOV / FOV盲区范围
}

function ObjSensorSample(session, time) {
    this.session = session; // Date or AppoxSessionIdentifier
    this.time = time; // [s] Time offset in session / Session内的相对时间
    this.objects = [];  // List of objects / 目标物列表
    this.cipvIndex = null;  // Index of CIPV object / 前向关键目标序号
    this.lkoIndex = null;  // Index of left side key object / 左侧关键目标序号
    this.rkoIndex = null;  // Index of right side key object / 右侧关键目标序号
    this.vehicleSpeed = null;  // [KPH] Subject vehicle's speed / 本车车速
    this.vehicleCurvature = null;  // [1/m] Subject vehicle's turning curvature / 本车曲率
    this.vehicleWidth = null;  // [m] Subject vehicle's width / 本车宽度
    this.vehicleLength = null;  // [m] Subject vehicle's length / 本车长度
    this.vehicleWheelBase = null;  // [m] Subject vehicle's wheel base / 本车轴距
    this.vehicleFrontOverhang = null;  // [m] Subject vehicle's front overhang / 本车前悬
    this.fovs = [];  // List of FOV / FOV列表
    this.getTTC1 = function (obj) { // Calculate first order TTC / 计算一阶TTC [s]
        if (obj.vxRel != null) {
            var vxRelM = obj.vxRel / 3.6;
            if (obj.cpx > 0) {
                if (vxRelM >= -0.1) return 20;
                else return Math.min(20, -(obj.cpx / vxRelM));
            }
            else if (this.vehicleLength != null && obj.cpx < -this.vehicleLength) {
                if (vxRelM <= 0.1) return 20;
                else return Math.min(20, (-this.vehicleLength - obj.cpx) / vxRelM);
            }
            else return null;
        }
        else return null;
    }
    this.getTTC2 = function (obj) { // Calculate second order TTC / 计算二阶TTC [s]
        if (obj.vxRel != null && obj.axRel != null) {
            var vxRelM = obj.vxRel / 3.6;
            var axRel = obj.axRel;
            if (obj.cpx > 0) {
                if (vxRelM >= -0.1) return 20;
                else if (Math.abs(axRel) < 0.1) return Math.min(20, -(obj.cpx / vxRelM));
                else if (vxRelM * vxRelM - 2 * axRel * obj.cpx >= 0) return Math.min(20, (-vxRelM - Math.sqrt(vxRelM * vxRelM - 2 * axRel * obj.cpx)) / axRel); //0.5at^2 + vt - d = 0
                else return 20;
            }
            else if (this.vehicleLength != null && obj.cpx < -this.vehicleLength) {
                var dist = -this.vehicleLength - obj.cpx;
                if (vxRelM <= 0.1) return 20;
                else if (Math.abs(axRel) < 0.1) return Math.min(20, (-this.vehicleLength - obj.cpx) / vxRelM);
                else if (vxRelM * vxRelM + 2 * axRel * dist >= 0) return Math.min(20, (-vxRelM + Math.sqrt(vxRelM * vxRelM + 2 * axRel * dist)) / axRel);
                return 20;
            }
            else return null;
        }
        else return null;
    }
    this.getTHW = function (obj) { // Calculate time headway / 计算车间时距 [s]
        if (obj.cpx > 0 && this.vehicleSpeed != null) {
            var vxEgoM = this.vehicleSpeed / 3.6;
            if (vxEgoM <= 0.1) return 20;
            else return Math.min(20, obj.cpx / vxEgoM);
        }
        else return null;
    }
    this.getDCA = function (obj, reactionTime) { // Calculate DCA / 计算避免碰撞最小减速度 [m/s²] reactionTime=0: ODCA, >0: PDCA
        if (obj.cpx > 0 && this.vehicleSpeed != null && obj.vxAbs != null && obj.axAbs != null) {
            var vxEgoM = this.vehicleSpeed / 3.6;
            var vxAbsM = obj.vxAbs / 3.6;
            var axAbs = obj.axAbs;
            var egoDxReaction = vxEgoM * reactionTime;
            var objDxReaction = vxAbsM * reactionTime + 0.5 * axAbs * reactionTime * reactionTime;
            var dxRelAR = obj.cpx - 0.1 + objDxReaction - egoDxReaction;
            var egoVxAbsAR = vxEgoM;
            var objVxAbsAR = vxAbsM + axAbs * reactionTime;
            var objVxRelAR = vxAbsM - vxEgoM;
            var objAxAbsAR = axAbs;
            if (dxRelAR <= 0) return -20;
            if (objAxAbsAR >= 0) {
                if (objVxRelAR >= 0) return 0;
                else return Math.max(-20, objAxAbsAR - objVxRelAR * objVxRelAR / (2 * dxRelAR));
            }
            else {
                if (objVxRelAR >= 0) return Math.max(-20, egoVxAbsAR * egoVxAbsAR / (objVxAbsAR * objVxAbsAR / objAxAbsAR - 2 * dxRelAR));
                if (objVxRelAR * objVxRelAR / (2 * dxRelAR) < objAxAbsAR) return Math.max(-20, egoVxAbsAR * egoVxAbsAR / (objVxAbsAR * objVxAbsAR / objAxAbsAR - 2 * dxRelAR));
                else return Math.Max(-20, objAxAbsAR - objVxRelAR * objVxRelAR / (2 * dxRelAR));
            }
        }
        else return null;
    }
    this.calBoxCorners = function (obj, coef) { // 8 points are connected sequentially, and "coef" is the reduction ratio of inclined points (generally 0.9~1) / 8个点顺序连接, coef为斜方向点缩小比例(一般为0.9~1)
        var posx = obj.posX;
        var posy = obj.posY;
        var width = obj.width
        var length = obj.length;
        var heading = obj.heading != null ? obj.heading : 0;
        var pts = Array(8);
        pts[0] = new BIPoint(length / 2, 0);
        pts[1] = new BIPoint((length / 2) * coef, (width / 2) * coef);
        pts[2] = new BIPoint(0, width / 2);
        pts[3] = new BIPoint((-length / 2) * coef, (width / 2) * coef);
        pts[4] = new BIPoint(-length / 2, 0);
        pts[5] = new BIPoint((-length / 2) * coef, (-width / 2) * coef);
        pts[6] = new BIPoint(0, -width / 2);
        pts[7] = new BIPoint((length / 2) * coef, (-width / 2) * coef);
        var deg2rad = Math.PI / 180;
        var coshd = Math.cos(heading * deg2rad);
        var sinhd = Math.sin(heading * deg2rad);
        for (var i = 0; i < 8; i++) {
            var pt = pts[i];
            var x = coshd * pt.x - sinhd * pt.y;
            var y = sinhd * pt.x + coshd * pt.y;
            pt.x = posx + x;
            pt.y = posy + y;
        }
        return pts;
    }
    this.getOverlap = function (obj, boxCornerRatio) { // Calculate overlap / 计算重叠率 [%]
        if (this.vehicleCurvature != null && this.vehicleWidth != null) {
            var widthEgo = this.vehicleWidth;
            var curvEgo = this.vehicleCurvature;
            var fo = this.vehicleFrontOverhang == null ? 0.9 : this.vehicleFrontOverhang;
            var wb = this.vehicleWheelBase == null ? 2.8 : this.vehicleWheelBase;
            if (obj.posX <= 0 || widthEgo <= 0) return null;
            var widthObj = 0;
            if (obj.posMode == PositionMode.ClosestPoint) {
                switch (obj.category()) {
                    case ObjectClass.Car:
                        widthObj = 1.9;
                        break;
                    case ObjectClass.Pedestrian:
                        widthObj = 0.5;
                        break;
                    case ObjectClass.Truck:
                        widthObj = 2.1;
                        break;
                    case ObjectClass.Bike:
                        widthObj = 0.6;
                        break;
                    case ObjectClass.RailCar:
                        widthObj = 2.6;
                        break;
                    default:
                        widthObj = 1.0;
                        break;
                }
            }
            else { //BoxCenter
                var corners = this.calBoxCorners(obj, boxCornerRatio);
                var miny = Number.POSITIVE_INFINITY, maxy = Number.NEGATIVE_INFINITY;
                for (var i = 0; i < corners.length; i++) {
                    miny = Math.min(miny, corners[i].y);
                    maxy = Math.max(maxy, corners[i].y);
                }
                widthObj = maxy - miny;
            }
            var dyEgo = 0.5 * curvEgo * obj.posX * obj.posX + curvEgo * (fo + wb) * obj.posX;
            var dyObj = obj.posY;
            var overLap = (Math.min(dyEgo + 0.5 * widthEgo, dyObj + 0.5 * widthObj) - Math.max(dyEgo - 0.5 * widthEgo, dyObj - 0.5 * widthObj));
            var overLapRate = overLap / (overLap >= 0 ? Math.min(widthEgo, widthObj) : widthEgo);
            return Math.max(-3.0, overLapRate) * 100;
        }
        else return null;
    }
    this.getLC = function (obj, boxCornerRatio) { // Calculate lateral clearance / 计算横向间距 [m]
        if (this.vehicleWidth != null) {
            if (obj.posMode == PositionMode.ClosestPoint) {
                if (obj.posX > 0) return null;
                else if (obj.posY > 0) return obj.posY - this.vehicleWidth * 0.5;
                else return -this.vehicleWidth * 0.5 - obj.posY;
            }
            else { // BoxCenter
                var points = this.calBoxCorners(obj, boxCornerRatio);
                var minx = Number.POSITIVE_INFINITY;
                var miny = Number.POSITIVE_INFINITY, maxy = Number.NEGATIVE_INFINITY;
                for (var i = 0; i < points.length; i++) {
                    var pt = points[i];
                    if (pt.x < minx) minx = pt.x;
                    if (pt.y < miny) miny = pt.y;
                    if (pt.y > maxy) maxy = pt.y;
                }
                if (minx > 0) return null;
                else if (miny > 0) return miny - this.vehicleWidth * 0.5;
                else if (maxy < 0) return -this.vehicleWidth * 0.5 - maxy;
                else return null;
            }
        }
        else return null;
    }
    this.getRC = function (obj, boxCornerRatio) { // Calculate rear clearance / 计算后向间距 [m]
        if (this.vehicleLength != null) {
            var pts = [];
            if (obj.posMode == PositionMode.ClosestPoint) pts = [ new BIPoint(obj.posX, obj.posY) ];
            else pts = this.calBoxCorners(obj, boxCornerRatio);
            var maxx = Number.NEGATIVE_INFINITY;
            for (var i = 0; i < pts.length; i++) {
                var pt = pts[i];
                if (pt.x > maxx) maxx = pt.x;
            }
            if (maxx > 0) return null;
            else return -this.vehicleLength - maxx;
        }
        else return null;
    }
}

function convObjSensorSampleV5(gs) {
    if (gs.values.length < 11) return null;
    if (gs.values[0] == null || gs.values[1] == null || gs.values[2] == null) return null;

    var nObjects = gs.values[0];
    var nTrajectories = gs.values[1];
    var nContours = gs.values[2];
    var nFovs = gs.values[10] == null ? 0 : gs.values[10];
    var sizeWithExtra = 11 + nObjects * 42 + nFovs * 6 + (nTrajectories + nContours) * 2;
    var sizeWithoutExtra = 11 + nObjects * 42 + nFovs * 6;
    if (gs.values.length != sizeWithExtra && gs.values.length != sizeWithoutExtra) return null;

    var sample = new ObjSensorSample(gs.session, gs.time);
    sample.cipvIndex = gs.values[3];
    sample.lkoIndex = gs.values[4];
    sample.rkoIndex = gs.values[5];
    sample.vehicleSpeed = gs.values[6];
    sample.vehicleCurvature = gs.values[7];
    sample.vehicleWidth = gs.values[8];
    sample.vehicleLength = gs.values[9];

    var objBase = 11;
    var fovBase = objBase + 42 * nObjects;
    var trajBase = fovBase + 6 * nFovs;
    var contBase = trajBase + 2 * nTrajectories;
    for (var i = 0; i < nObjects; i++)
    {
        var obj = new ObjectInfo();
        var baseIndex = objBase + 42 * i;
        if (gs.values[baseIndex] != null) obj.id = gs.values[baseIndex];
        if (gs.values[baseIndex + 1] != null) obj.age = gs.values[baseIndex + 1];
        obj.rawID = gs.values[baseIndex + 2];
        obj.rawAge = gs.values[baseIndex + 3];
        obj.rawClassID = gs.values[baseIndex + 4];
        if (gs.values[baseIndex + 5] != null) obj.classification = gs.values[baseIndex + 5];
        if (gs.values[baseIndex + 6] != null) obj.posMode = gs.values[baseIndex + 6];
        if (gs.values[baseIndex + 7] != null) obj.posX = gs.values[baseIndex + 7];
        if (gs.values[baseIndex + 8] != null) obj.posY = gs.values[baseIndex + 8];
        if (gs.values[baseIndex + 9] != null) obj.cpx = gs.values[baseIndex + 9];
        if (gs.values[baseIndex + 10] != null) obj.cpy = gs.values[baseIndex + 10];
        if (gs.values[baseIndex + 11] != null) obj.cpd = gs.values[baseIndex + 11];
        obj.width = gs.values[baseIndex + 12];
        obj.length = gs.values[baseIndex + 13];
        obj.heading = gs.values[baseIndex + 14];
        obj.vxRel = gs.values[baseIndex + 15];
        obj.vxAbs = gs.values[baseIndex + 16];
        obj.vyRel = gs.values[baseIndex + 17];
        obj.vyAbs = gs.values[baseIndex + 18];
        obj.axRel = gs.values[baseIndex + 19];
        obj.axAbs = gs.values[baseIndex + 20];
        obj.ayRel = gs.values[baseIndex + 21];
        obj.ayAbs = gs.values[baseIndex + 22];
        obj.timeOffset = gs.values[baseIndex + 35];
        if (gs.values[baseIndex + 36] != null) {
            obj.color.valid = true;
            obj.color.r = gs.values[baseIndex + 36];
            obj.color.g = gs.values[baseIndex + 37];
            obj.color.b = gs.values[baseIndex + 38];
        }
        obj.classConfidence = gs.values[baseIndex + 39];
        obj.posXSigma = gs.values[baseIndex + 40];
        obj.posYSigma = gs.values[baseIndex + 41];

        if (gs.values.length == sizeWithExtra) {
            var trajOffset = gs.values[baseIndex + 31];
            var trajSize = gs.values[baseIndex + 32];
            if (trajOffset != null && trajSize != null) {
                obj.trajectory = new Array(trajSize);
                for (var n = 0; n < trajSize; n++) {
                    var trajX = gs.values[trajBase + 2 * trajOffset + 2 * n];
                    var trajY = gs.values[trajBase + 2 * trajOffset + 2 * n + 1];
                    obj.trajectory[n] = new BIPoint(trajX, trajY);
                }
            }
            var contOffset = gs.values[baseIndex + 33];
            var contSize = gs.values[baseIndex + 34];
            if (contOffset != null && contSize != null) {
                obj.contour = new Array(contSize);
                for (var n = 0; n < contSize; n++) {
                    var contX = gs.values[contBase + 2 * contOffset + 2 * n];
                    var contY = gs.values[contBase + 2 * contOffset + 2 * n + 1];
                    obj.contour[n] = new BIPoint(contX, contY);
                }
            }
        }

        sample.objects.push(obj);
    }

    for (var i = 0; i < nFovs; i++)
    {
        var fov = new ObjectSensorFov();
        var baseIndex = fovBase + 6 * i;
        if (gs.values[baseIndex] != null) fov.positionX = gs.values[baseIndex];
        if (gs.values[baseIndex + 1] != null) fov.positionY = gs.values[baseIndex + 1];
        if (gs.values[baseIndex + 2] != null) fov.orientation = gs.values[baseIndex + 2];
        if (gs.values[baseIndex + 3] != null) fov.angleRange = gs.values[baseIndex + 3];
        if (gs.values[baseIndex + 4] != null) fov.distanceRange = gs.values[baseIndex + 4];
        if (gs.values[baseIndex + 5] != null) fov.blindRange = gs.values[baseIndex + 5];
        sample.fovs.push(fov);
    }

    return sample;
}

function convObjSensorSampleV6(gs) {
    if (gs.values.length < 16) return null;
    if (gs.values[0] == null || gs.values[1] == null || gs.values[2] == null || gs.values[3] == null) return null;

    var nObjects = gs.values[0];
    var nFovs = gs.values[1];
    var nTrajectories = gs.values[2];
    var nContours = gs.values[3];
    var sizeWithExtra = 16 + nObjects * 52 + nFovs * 6 + (nTrajectories + nContours) * 2;
    var sizeWithoutExtra = 16 + nObjects * 52 + nFovs * 6;
    if (gs.values.length != sizeWithExtra && gs.values.length != sizeWithoutExtra) return null;

    var sample = new ObjSensorSample(gs.session, gs.time);
    sample.cipvIndex = gs.values[4];
    sample.lkoIndex = gs.values[5];
    sample.rkoIndex = gs.values[6];
    sample.vehicleSpeed = gs.values[7];
    sample.vehicleCurvature = gs.values[8];
    sample.vehicleWidth = gs.values[9];
    sample.vehicleLength = gs.values[10];
    sample.vehicleWheelBase = gs.values[11];
    sample.vehicleFrontOverhang = gs.values[12];

    var objBase = 16;
    var fovBase = objBase + 52 * nObjects;
    var trajBase = fovBase + 6 * nFovs;
    var contBase = trajBase + 2 * nTrajectories;
    for (var i = 0; i < nObjects; i++) {
        var obj = new ObjectInfo();
        var baseIndex = objBase + 52 * i;
        if (gs.values[baseIndex] != null) obj.id = gs.values[baseIndex];
        if (gs.values[baseIndex + 1] != null) obj.age = gs.values[baseIndex + 1];
        obj.rawID = gs.values[baseIndex + 2];
        obj.rawAge = gs.values[baseIndex + 3];
        obj.rawClassID = gs.values[baseIndex + 4];
        if (gs.values[baseIndex + 5] != null) obj.classification = gs.values[baseIndex + 5];
        if (gs.values[baseIndex + 6] != null) obj.posMode = gs.values[baseIndex + 6];
        if (gs.values[baseIndex + 7] != null) obj.posX = gs.values[baseIndex + 7];
        if (gs.values[baseIndex + 8] != null) obj.posY = gs.values[baseIndex + 8];
        if (gs.values[baseIndex + 9] != null) obj.cpx = gs.values[baseIndex + 9];
        if (gs.values[baseIndex + 10] != null) obj.cpy = gs.values[baseIndex + 10];
        if (gs.values[baseIndex + 11] != null) obj.cpd = gs.values[baseIndex + 11];
        obj.width = gs.values[baseIndex + 12];
        obj.length = gs.values[baseIndex + 13];
        obj.heading = gs.values[baseIndex + 14];
        obj.vxRel = gs.values[baseIndex + 15];
        obj.vxAbs = gs.values[baseIndex + 16];
        obj.vyRel = gs.values[baseIndex + 17];
        obj.vyAbs = gs.values[baseIndex + 18];
        obj.axRel = gs.values[baseIndex + 19];
        obj.axAbs = gs.values[baseIndex + 20];
        obj.ayRel = gs.values[baseIndex + 21];
        obj.ayAbs = gs.values[baseIndex + 22];
        if (gs.values[baseIndex + 23] != null) {
            obj.color.valid = true;
            obj.color.r = gs.values[baseIndex + 23];
            obj.color.g = gs.values[baseIndex + 24];
            obj.color.b = gs.values[baseIndex + 25];
        }
        obj.timeOffset = gs.values[baseIndex + 26];
        obj.confidence = gs.values[baseIndex + 27];
        obj.classConfidence = gs.values[baseIndex + 28];
        obj.height = gs.values[baseIndex + 29];
        obj.posZ = gs.values[baseIndex + 30];
        obj.posXSigma = gs.values[baseIndex + 31];
        obj.posYSigma = gs.values[baseIndex + 32];
        obj.posYSigma = gs.values[baseIndex + 33];
        obj.rcs = gs.values[baseIndex + 42];
        obj.vLng = gs.values[baseIndex + 43];
        obj.vLat = gs.values[baseIndex + 44];
        obj.yawRate = gs.values[baseIndex + 45];
        obj.curvature = gs.values[baseIndex + 46];

        if (gs.values.length == sizeWithExtra) {
            var trajOffset = gs.values[baseIndex + 48];
            var trajSize = gs.values[baseIndex + 49];
            if (trajOffset != null && trajSize != null) {
                obj.trajectory = new Array(trajSize);
                for (var n = 0; n < trajSize; n++) {
                    var trajX = gs.values[trajBase + 2 * trajOffset + 2 * n];
                    var trajY = gs.values[trajBase + 2 * trajOffset + 2 * n + 1];
                    obj.trajectory[n] = new BIPoint(trajX, trajY);
                }
            }
            var contOffset = gs.values[baseIndex + 50];
            var contSize = gs.values[baseIndex + 51];
            if (contOffset != null && contSize != null) {
                obj.contour = new Array(contSize);
                for (var n = 0; n < contSize; n++) {
                    var contX = gs.values[contBase + 2 * contOffset + 2 * n];
                    var contY = gs.values[contBase + 2 * contOffset + 2 * n + 1];
                    obj.contour[n] = new BIPoint(contX, contY);
                }
            }
        }

        sample.objects.push(obj);
    }

    for (var i = 0; i < nFovs; i++) {
        var fov = new ObjectSensorFov();
        var baseIndex = fovBase + 6 * i;
        if (gs.values[baseIndex] != null) fov.positionX = gs.values[baseIndex];
        if (gs.values[baseIndex + 1] != null) fov.positionY = gs.values[baseIndex + 1];
        if (gs.values[baseIndex + 2] != null) fov.orientation = gs.values[baseIndex + 2];
        if (gs.values[baseIndex + 3] != null) fov.angleRange = gs.values[baseIndex + 3];
        if (gs.values[baseIndex + 4] != null) fov.distanceRange = gs.values[baseIndex + 4];
        if (gs.values[baseIndex + 5] != null) fov.blindRange = gs.values[baseIndex + 5];
        sample.fovs.push(fov);
    }

    return sample;
}

function convObjSensorSampleV7(gs) {
    if (gs.values.length < 20) return null;
    if (gs.values[0] == null || gs.values[1] == null || gs.values[2] == null || gs.values[3] == null) return null;

    var nObjects = gs.values[0];
    var nFovs = gs.values[1];
    var nTrajectories = gs.values[2];
    var nContours = gs.values[3];
    var sizeWithExtra = 20 + nObjects * 72 + nFovs * 6 + (nTrajectories + nContours) * 2;
    var sizeWithoutExtra = 20 + nObjects * 72 + nFovs * 6;
    if (gs.values.length != sizeWithExtra && gs.values.length != sizeWithoutExtra) return null;

    var sample = new ObjSensorSample(gs.session, gs.time);
    sample.cipvIndex = gs.values[4];
    sample.lkoIndex = gs.values[5];
    sample.rkoIndex = gs.values[6];
    sample.vehicleSpeed = gs.values[7];
    sample.vehicleCurvature = gs.values[8];
    sample.vehicleWidth = gs.values[9];
    sample.vehicleLength = gs.values[10];
    sample.vehicleWheelBase = gs.values[11];
    sample.vehicleFrontOverhang = gs.values[12];

    var objBase = 20;
    var fovBase = objBase + 72 * nObjects;
    var trajBase = fovBase + 6 * nFovs;
    var contBase = trajBase + 2 * nTrajectories;
    for (var i = 0; i < nObjects; i++) {
        var obj = new ObjectInfo();
        var baseIndex = objBase + 72 * i;
        if (gs.values[baseIndex] != null) obj.id = gs.values[baseIndex];
        if (gs.values[baseIndex + 1] != null) obj.age = gs.values[baseIndex + 1];
        obj.rawID = gs.values[baseIndex + 2];
        obj.rawAge = gs.values[baseIndex + 3];
        obj.rawClassID = gs.values[baseIndex + 4];
        if (gs.values[baseIndex + 5] != null) obj.classification = gs.values[baseIndex + 5];
        if (gs.values[baseIndex + 6] != null) obj.posMode = gs.values[baseIndex + 6];
        if (gs.values[baseIndex + 7] != null) obj.posX = gs.values[baseIndex + 7];
        if (gs.values[baseIndex + 8] != null) obj.posY = gs.values[baseIndex + 8];
        obj.posZ = gs.values[baseIndex + 9];
        obj.posXSigma = gs.values[baseIndex + 10];
        obj.posYSigma = gs.values[baseIndex + 11];
        obj.posYSigma = gs.values[baseIndex + 12];
        if (gs.values[baseIndex + 13] != null) obj.cpx = gs.values[baseIndex + 13];
        if (gs.values[baseIndex + 14] != null) obj.cpy = gs.values[baseIndex + 14];
        if (gs.values[baseIndex + 15] != null) obj.cpd = gs.values[baseIndex + 15];
        obj.width = gs.values[baseIndex + 16];
        obj.length = gs.values[baseIndex + 17];
        obj.height = gs.values[baseIndex + 18];
        obj.heading = gs.values[baseIndex + 19];
        obj.vxRel = gs.values[baseIndex + 20];
        obj.vxAbs = gs.values[baseIndex + 21];
        obj.vyRel = gs.values[baseIndex + 22];
        obj.vyAbs = gs.values[baseIndex + 23];
        obj.vzRel = gs.values[baseIndex + 24];
        obj.vzAbs = gs.values[baseIndex + 25];
        obj.axRel = gs.values[baseIndex + 26];
        obj.axAbs = gs.values[baseIndex + 27];
        obj.ayRel = gs.values[baseIndex + 28];
        obj.ayAbs = gs.values[baseIndex + 29];
        obj.azRel = gs.values[baseIndex + 30];
        obj.azAbs = gs.values[baseIndex + 31];
        if (gs.values[baseIndex + 32] != null) {
            obj.color.valid = true;
            obj.color.r = gs.values[baseIndex + 32];
            obj.color.g = gs.values[baseIndex + 33];
            obj.color.b = gs.values[baseIndex + 34];
        }
        obj.timeOffset = gs.values[baseIndex + 35];
        obj.confidence = gs.values[baseIndex + 36];
        obj.classConfidence = gs.values[baseIndex + 37];
        obj.rcs = gs.values[baseIndex + 46];
        obj.vLng = gs.values[baseIndex + 47];
        obj.vLat = gs.values[baseIndex + 48];
        obj.yawRate = gs.values[baseIndex + 49];
        obj.curvature = gs.values[baseIndex + 50];

        if (gs.values.length == sizeWithExtra) {
            var trajOffset = gs.values[baseIndex + 68];
            var trajSize = gs.values[baseIndex + 69];
            if (trajOffset != null && trajSize != null) {
                obj.trajectory = new Array(trajSize);
                for (var n = 0; n < trajSize; n++) {
                    var trajX = gs.values[trajBase + 2 * trajOffset + 2 * n];
                    var trajY = gs.values[trajBase + 2 * trajOffset + 2 * n + 1];
                    obj.trajectory[n] = new BIPoint(trajX, trajY);
                }
            }
            var contOffset = gs.values[baseIndex + 70];
            var contSize = gs.values[baseIndex + 71];
            if (contOffset != null && contSize != null) {
                obj.contour = new Array(contSize);
                for (var n = 0; n < contSize; n++) {
                    var contX = gs.values[contBase + 2 * contOffset + 2 * n];
                    var contY = gs.values[contBase + 2 * contOffset + 2 * n + 1];
                    obj.contour[n] = new BIPoint(contX, contY);
                }
            }
        }

        sample.objects.push(obj);
    }

    for (var i = 0; i < nFovs; i++) {
        var fov = new ObjectSensorFov();
        var baseIndex = fovBase + 6 * i;
        if (gs.values[baseIndex] != null) fov.positionX = gs.values[baseIndex];
        if (gs.values[baseIndex + 1] != null) fov.positionY = gs.values[baseIndex + 1];
        if (gs.values[baseIndex + 2] != null) fov.orientation = gs.values[baseIndex + 2];
        if (gs.values[baseIndex + 3] != null) fov.angleRange = gs.values[baseIndex + 3];
        if (gs.values[baseIndex + 4] != null) fov.distanceRange = gs.values[baseIndex + 4];
        if (gs.values[baseIndex + 5] != null) fov.blindRange = gs.values[baseIndex + 5];
        sample.fovs.push(fov);
    }

    return sample;
}

// Interpolator / 样本插值函数
function interpolateObjSensorSample(session, time, s1, w1, s2, w2) {
    var sample = new ObjSensorSample(session, time);
    sample.vehicleWidth = s1.vehicleWidth;
    sample.vehicleLength = s1.vehicleLength;
    sample.vehicleSpeed = s1.vehicleSpeed == null || s2.vehicleSpeed == null ? null : s1.vehicleSpeed * w1 + s2.vehicleSpeed * w2;
    sample.vehicleCurvature = s1.vehicleCurvature == null || s2.vehicleCurvature == null ? null : s1.vehicleCurvature * w1 + s2.vehicleCurvature * w2;
    sample.vehicleWheelBase = s1.vehicleWheelBase;
    sample.vehicleFrontOverhang = s1.vehicleFrontOverhang;
    sample.fovs = s1.fovs;

    var cipvID = null, lkoID = null, rkoID = null;
    if (s1.cipvIndex != null && s2.cipvIndex != null && s1.objects[s1.cipvIndex].id == s2.objects[s2.cipvIndex].id) {
        cipvID = s1.objects[s1.cipvIndex].id;
    }
    if (s1.lkoIndex != null && s2.lkoIndex != null && s1.objects[s1.lkoIndex].id == s2.objects[s2.lkoIndex].id) {
        lkoID = s1.objects[s1.lkoIndex].id;
    }
    if (s1.rkoIndex != null && s2.rkoIndex != null && s1.objects[s1.rkoIndex].id == s2.objects[s2.rkoIndex].id) {
        rkoID = s1.objects[s1.rkoIndex].id;
    }

    var index = 0;
    for (var i1 = 0; i1 < s1.objects.length; i1++) {
        var o1 = s1.objects[i1];
        var id = o1.id;
        for (var i2 = 0; i2 < s2.objects.length; i2++) {
            var o2 = s2.objects[i2];
            if (o2.id != id) continue;
            if (cipvID != null && cipvID == id) sample.cipvIndex = index;
            if (lkoID != null && lkoID == id) sample.lkoIndex = index;
            if (rkoID != null && rkoID == id) sample.rkoIndex = index;

            var obj = new ObjectInfo();
            obj.id = id;
            obj.age = o1.age;
            obj.rawID = o1.rawID;
            obj.rawAge = o1.RawAge;
            obj.confidence = o1.confidence == null || o2.confidence == null ? null : o1.confidence * w1 + o2.confidence * w2;
            obj.timeOffset = o1.timeOffset == null || o2.timeOffset == null ? null : o1.timeOffset * w1 + o2.timeOffset * w2;
            obj.color = w1 > w2 ? o1.color : o2.color;
            obj.classification = w1 > w2 ? o1.classification : o2.classification;
            obj.rawClassID = w1 > w2 ? o1.rawClassID : o2.rawClassID;
            obj.classConfidence = o1.classConfidence == null || o2.classConfidence == null ? null : o1.classConfidence * w1 + o2.classConfidence * w2;
            obj.posMode = o1.posMode;
            obj.posX = o1.posX * w1 + o2.posX * w2;
            obj.posY = o1.posY * w1 + o2.posY * w2;
            obj.posZ = o1.posZ == null || o2.posZ == null ? null : o1.posZ * w1 + o2.posZ * w2;
            obj.posXSigma = o1.posXSigma == null || o2.posXSigma == null ? null : o1.posXSigma * w1 + o2.posXSigma * w2;
            obj.posYSigma = o1.posYSigma == null || o2.posYSigma == null ? null : o1.posYSigma * w1 + o2.posYSigma * w2;
            obj.posZSigma = o1.posZSigma == null || o2.posZSigma == null ? null : o1.posZSigma * w1 + o2.posZSigma * w2;
            obj.cpx = o1.cpx * w1 + o2.cpx * w2;
            obj.cpy = o1.cpy * w1 + o2.cpy * w2;
            obj.cpd = o1.cpd * w1 + o2.cpd * w2;
            obj.vxAbs = o1.vxAbs == null || o2.vxAbs == null ? null : o1.vxAbs * w1 + o2.vxAbs * w2;
            obj.vyAbs = o1.vyAbs == null || o2.vyAbs == null ? null : o1.vyAbs * w1 + o2.vyAbs * w2;
            obj.vzAbs = o1.vzAbs == null || o2.vzAbs == null ? null : o1.vzAbs * w1 + o2.vzAbs * w2;
            obj.axAbs = o1.axAbs == null || o2.axAbs == null ? null : o1.axAbs * w1 + o2.axAbs * w2;
            obj.ayAbs = o1.ayAbs == null || o2.ayAbs == null ? null : o1.ayAbs * w1 + o2.ayAbs * w2;
            obj.azAbs = o1.azAbs == null || o2.azAbs == null ? null : o1.azAbs * w1 + o2.azAbs * w2;
            obj.vxRel = o1.vxRel == null || o2.vxRel == null ? null : o1.vxRel * w1 + o2.vxRel * w2;
            obj.vyRel = o1.vyRel == null || o2.vyRel == null ? null : o1.vyRel * w1 + o2.vyRel * w2;
            obj.vzRel = o1.vzRel == null || o2.vzRel == null ? null : o1.vzRel * w1 + o2.vzRel * w2;
            obj.axRel = o1.axRel == null || o2.axRel == null ? null : o1.axRel * w1 + o2.axRel * w2;
            obj.ayRel = o1.ayRel == null || o2.ayRel == null ? null : o1.ayRel * w1 + o2.ayRel * w2;
            obj.azRel = o1.azRel == null || o2.azRel == null ? null : o1.azRel * w1 + o2.azRel * w2;
            obj.rcs = o1.rcs == null || o2.rcs == null ? null : o1.rcs * w1 + o2.rcs * w2;
            obj.vLng = o1.vLng == null || o2.vLng == null ? null : o1.vLng * w1 + o2.vLng * w2;
            obj.vLat = o1.vLat == null || o2.vLat == null ? null : o1.vLat * w1 + o2.vLat * w2;
            obj.yawRate = o1.yawRate == null || o2.yawRate == null ? null : o1.yawRate * w1 + o2.yawRate * w2;
            obj.curvature = o1.curvature == null || o2.curvature == null ? null : o1.curvature * w1 + o2.curvature * w2;
            if (o1.heading != null && o2.heading != null) {
                var deg2rad = Math.PI / 180;
                var x1 = Math.cos(o1.heading * deg2rad);
                var y1 = Math.sin(o1.heading * deg2rad);
                var x2 = Math.cos(o2.heading * deg2rad);
                var y2 = Math.sin(o2.heading * deg2rad);
                var xo = x1 * w1 + x2 * w2;
                var yo = y1 * w1 + y2 * w2;
                if (xo != 0 || yo != 0) {
                    obj.heading = Math.atan2(yo, xo) / deg2rad;
                }
            }
            obj.width = o1.width == null || o2.width == null ? null : o1.width * w1 + o2.width * w2;
            obj.length = o1.length == null || o2.length == null ? null : o1.length * w1 + o2.length * w2;
            obj.height = o1.height == null || o2.height == null ? null : o1.height * w1 + o2.height * w2;
            obj.trajectory = w1 > w2 ? o1.trajectory : o2.trajectory;
            obj.contour = w1 > w2 ? o1.contour : o2.contour;
            sample.objects.push(obj);
            index++;
        }
    }

    return sample;
}

// Supported protocols / 目标物传感器数据样本支持解析的协议
var objSensorSampleProtocols = [
    "obj-sensor-sample-v5",
    "obj-sensor-sample-v6",
    "obj-sensor-sample-v7",
];

// Converters / 通用样本转换函数表
var objSensorSampleConverters = {
    "obj-sensor-sample-v5": convObjSensorSampleV5,
    "obj-sensor-sample-v6": convObjSensorSampleV6,
    "obj-sensor-sample-v7": convObjSensorSampleV7,
};

// Use this function to query object sensor sample at target time. If "interpolated" is true, the interpolated frame is preferred, otherwise the nearest frame / 获取指定session指定时刻的目标物传感器样本，若interpolated为true则优先取插值帧，否则固定为最近帧
function getObjSensorSample(channel, session, time, interpolated) {
    var s1 = null, w1 = null, s2 = null, w2 = null;
    var pairV7 = biGetGeneralSamplePair('obj-sensor-sample-v7', channel, session, time, 1);
    var pairV6 = biGetGeneralSamplePair('obj-sensor-sample-v6', channel, session, time, 1);
    var pairV5 = biGetGeneralSamplePair('obj-sensor-sample-v5', channel, session, time, 1);
    if (pairV7 != null) {
        if (pairV7.sample1 != null) s1 = convObjSensorSampleV7(pairV7.sample1);
        if (pairV7.sample2 != null) s2 = convObjSensorSampleV7(pairV7.sample2);
        w1 = pairV7.weight1;
        w2 = pairV7.weight2;
    } else if (pairV6 != null) {
        if (pairV6.sample1 != null) s1 = convObjSensorSampleV6(pairV6.sample1);
        if (pairV6.sample2 != null) s2 = convObjSensorSampleV6(pairV6.sample2);
        w1 = pairV6.weight1;
        w2 = pairV6.weight2;
    } else if (pairV5 != null) {
        if (pairV5.sample1 != null) s1 = convObjSensorSampleV5(pairV5.sample1);
        if (pairV5.sample2 != null) s2 = convObjSensorSampleV5(pairV5.sample2);
        w1 = pairV5.weight1;
        w2 = pairV5.weight2;
    } else return null;
    if (s1 == null || s2 == null) {
        return s1 == null ? s2 : s1;
    }
    if (interpolated) {
        return interpolateObjSensorSample(session, time, s1, w1, s2, w2);
    } else {
        return w1 > w2 ? s1 : s2;
    }
}